#include "epuckDriver/epuckDriver.hpp"
#include <string>
#include <cstddef>

EpuckDriver::EpuckDriver(std::string iD,std::string port):exec(false),EXPECTED_EPUCK_SIDE_VERSION(300){
 
    std::string strSerialPort(port.c_str());
    this->serialPort = new SerialPort(strSerialPort);

    this->epuckPubOdometry += "epuckOdometry";
    this->epuckPubOdometry += iD.c_str();    
    this->epuckPubIR += "epuckIR";
    this->epuckPubIR += iD.c_str();    
    this->epuckPubCamera += "epuckCamera";
    this->epuckPubCamera += iD.c_str();    

    this->epuckSubVelocity += "epuckVelocity";
    this->epuckSubVelocity += iD.c_str();
    this->epuckSubCommand += "epuckCommand";
    this->epuckSubCommand += iD.c_str();
    this->epuckSubFrontLED += "epuckFrontLED";
    this->epuckSubFrontLED += iD.c_str();
    this->epuckSubBodyLED += "epuckBodyLED";
    this->epuckSubBodyLED += iD.c_str();
    this->epuckSubRingLED += "epuckRingLED";
    this->epuckSubRingLED += iD.c_str();

    this->epuckServerSetOdom += "epuckSetOdom";
    this->epuckServerSetOdom += iD.c_str();
    this->epuckServerResetOdom += "epuckResetOdom";
    this->epuckServerResetOdom += iD.c_str();
    this->epuckServerGetGeom += "epuckGetGeom";
    this->epuckServerGetGeom += iD.c_str();
    this->epuckServerGetIRPose += "epuckGetIRPose";
    this->epuckServerGetIRPose += iD.c_str();
    this->epuckServerGetOdom += "epuckGetOdom";
    this->epuckServerGetOdom += iD.c_str();

    this->epuckPosition2d.reset(new EpuckPosition2d(this->serialPort));
    
    this->epuckIR.reset(new EpuckIR(this->serialPort));
   
    if( !this->epuckLEDs.get() ) 
        this->epuckLEDs.reset(new EpuckLEDs(this->serialPort));

    int sensor_x1 = 240;
    int sensor_y1 = 160;
    int sensor_width = 160;
    int sensor_height = 160;
    int zoom_fact_width = 4;
    int zoom_fact_height = 4;
    EpuckCamera::ColorModes color_mode = EpuckCamera::GREY_SCALE_MODE;    

    this->epuckCamera.reset(new EpuckCamera(this->serialPort,
                            sensor_x1,sensor_y1,
                            sensor_width, sensor_height,
                            zoom_fact_width, zoom_fact_height,
                            color_mode));
}

EpuckDriver::~EpuckDriver(){
    delete serialPort;
}

int EpuckDriver::Setup(){
   
    ROS_INFO("Serial Port"); 
    if(this->serialPort->initialize() == -1){
        ROS_ERROR("%s",this->serialPort->getError().c_str());
        return -1;    
    }   

    ROS_INFO("Epuck Respost"); 
    this->serialPort->sendInt(0x01);
    if(this->serialPort->recvUnsigned() != this->EXPECTED_EPUCK_SIDE_VERSION){
        ROS_ERROR("The e-puck side program version isn't the expected");
        return -1;
    }
  
    ROS_INFO("Camera Configuration"); 
    if(this->epuckCamera.get()){
        try{
            this->epuckCamera->Initialize();
            std::string version = this->epuckCamera->GetCameraVersion();
            ROS_INFO("E-puck camera initialized. Camera version: %s",version.c_str());
        }
        catch(std::exception &e){
            ROS_ERROR("%s",e.what());
            return -1;
        }
    }
    
    ROS_INFO("All setup ok"); 
    return 0;
}

void EpuckDriver::Init(){
    
    EpuckIR::IRData epuckIRData;
    if( this->epuckIR.get() ){
        epuckIRData = this->epuckIR->GetIRData();
        
        this->irData.voltages_count = epuckIRData.voltages.size();
        this->irData.voltages.resize(this->irData.voltages_count);
        
        this->irData.ranges_count = epuckIRData.ranges.size();
        this->irData.ranges.resize(this->irData.ranges_count);
    }

    if( this->epuckCamera.get() ){
        unsigned imageWidth, imageHeight;
        EpuckCamera::ColorModes colorMode;
        this->epuckCamera->GetCameraData(imageWidth,imageHeight,colorMode);

        this->cameraData.width = imageWidth;
        this->cameraData.height = imageHeight;
            
        switch(colorMode){

            case EpuckCamera::GREY_SCALE_MODE:
                this->cameraData.bpp = 8;
                this->cameraData.format = 1;
                break;

            case EpuckCamera::RGB_565_MODE:
                this->cameraData.bpp = 16;
                this->cameraData.format = 4;
                break;

            case EpuckCamera::YUV_MODE:
                this->cameraData.bpp = 16;
                this->cameraData.format = 2;
                break;
        }
        
        this->cameraData.fdiv = 1;
        this->cameraData.compression = 0;
        this->cameraData.image_count = this->cameraData.width*this->cameraData.height*this->cameraData.bpp/8;
        this->cameraData.image.resize(this->cameraData.image_count);
    }

    this->pubOdometry = this->n_epuck.advertise<epuckDriver::OdometryPublisherMsg>(epuckPubOdometry.c_str(),1);
    this->pubIR =
this->n_epuck.advertise<epuckDriver::IRDataPublisherMsg>(epuckPubIR.c_str(),1);    
    this->pubCamera =
this->n_epuck.advertise<epuckDriver::CameraDataMsg>(epuckPubCamera.c_str(),1); 
}

int EpuckDriver::Start(){

    if(this->Setup() == -1 );
        // return -1;     
    
    this->Init(); 
    
    this->exec = true; 

    return 0;  
}

void EpuckDriver::Publish(bool vel,bool ir, bool cam){

    if(this->exec){
        
       
     if(vel && this->epuckPosition2d.get())  
        this->OdometryPublisher();

     if(ir && this->epuckIR.get())  
            this->IRPublisher();

     if(cam && this->epuckCamera.get())  
        this->CameraPublisher();
    }
    else
        ROS_INFO("Start() must be summoner");
}

/*----------------Position2D---------------------------------*/
void EpuckDriver::VelocityCallBack(const 
geometry_msgs::Twist::ConstPtr& twist) {

    double linearVelocity, angularVelocity;  

    linearVelocity = twist->linear.x;
    angularVelocity = twist->angular.z;

    epuckSignal.lock();
    this->epuckPosition2d->SetVel(linearVelocity,angularVelocity);
    epuckSignal.unlock();

    if( twist->linear.y){
        ROS_WARN("Ignored invalid Sideways velocity command");
    }
}

void EpuckDriver::CommandCallBack( const epuckDriver::CommandSubscriberMsg::ConstPtr& commandSubscriberMsg) {

    double linearVelocity, angular;
    
    linearVelocity = commandSubscriberMsg->twist.linear.x;
    angular = commandSubscriberMsg->pose2d.theta;

    epuckSignal.lock();
    this->epuckPosition2d->SetVel(linearVelocity,angular);
    epuckSignal.unlock();

    ROS_INFO("command ok");
}

void EpuckDriver::OdometryPublisher(){

    EpuckPosition2d::DynamicConfiguration epuckOdometry;

    epuckOdometry = this->epuckPosition2d->UpdateOdometry();

    this->pose2d.x = epuckOdometry.pose.x;
    this->pose2d.y = epuckOdometry.pose.y;
    this->pose2d.theta = epuckOdometry.pose.theta;

    this->twist.linear.x = epuckOdometry.velocity.x;
    this->twist.linear.y = epuckOdometry.velocity.y;
    this->twist.linear.z = 0;

    this->twist.angular.x = 0;
    this->twist.angular.y = 0;
    this->twist.angular.z = epuckOdometry.velocity.theta;

    epuckDriver::OdometryPublisherMsg pubOdometryMsg;

    pubOdometryMsg.pose2d = this->pose2d;
    pubOdometryMsg.twist  = this->twist;

    pubOdometry.publish(pubOdometryMsg);
}

bool EpuckDriver::ResetOdometry(epuckDriver::ResetOdometry::Request&req,
epuckDriver::ResetOdometry::Response& res ){
    
    ROS_INFO("RESET ODOM");
    if(this->exec){ 
        epuckSignal.lock();
        this->epuckPosition2d->ResetOdometry();
        epuckSignal.unlock();
    }else
        ROS_INFO("Start Fisrt !!!");
    
    return true;
}

bool EpuckDriver::GetOdometry(epuckDriver::GetOdometry::Request& req, 
epuckDriver::GetOdometry::Response &res){
    
    if(this->exec){ 
        epuckSignal.lock();
        
        EpuckPosition2d::DynamicConfiguration epuckOdometry;

        epuckOdometry = this->epuckPosition2d->UpdateOdometry();
        this->pose2d.x = epuckOdometry.pose.x;
        this->pose2d.y = epuckOdometry.pose.y;
        this->pose2d.theta = epuckOdometry.pose.theta;

        res.pose2d = pose2d; 

        epuckSignal.unlock();
    }else
        ROS_INFO("Start Fisrt !!!");
    
    return true;
}

bool EpuckDriver::SetOdometry(epuckDriver::SetOdometry::Request &req, epuckDriver::SetOdometry::Response &res){
   
    ROS_INFO("SET ODOM");

    EpuckInterface::Triple epuckOdom;
    epuckOdom.x = req.pose2d.x;
    epuckOdom.y = req.pose2d.y;
    epuckOdom.theta = req.pose2d.theta;

    if(this->exec){
        epuckSignal.lock();
        this->epuckPosition2d->SetOdometry(epuckOdom);
        epuckSignal.unlock();
    }else
        ROS_INFO("Start Fisrt !!!");
    
    return true;
}

bool EpuckDriver::GetGeometry(epuckDriver::GetGeometry::Request &req,
epuckDriver::GetGeometry::Response &res){
  
    ROS_INFO("GET GEOMETRY");
    if(this->exec) { 
        EpuckPosition2d::BodyGeometry epuckGeom =
            this->epuckPosition2d->GetGeometry();

        res.width = epuckGeom.width;
        res.height = epuckGeom.height;
    }
    else
        ROS_INFO("Start First !!!");
    
    return true;
}
/*---------------------------------------------------------------*/

/*---------------------IR---------------------------------*/
void EpuckDriver::IRPublisher(){
    
    EpuckIR::IRData epuckIRData;
    epuckIRData = this->epuckIR->GetIRData();

    std::vector<float>::iterator itVoltages,itRanges;
    itVoltages = epuckIRData.voltages.begin();
    itRanges   = epuckIRData.ranges.begin();    
  
    for(int count = 0;itVoltages < epuckIRData.voltages.end();
        itVoltages++,itRanges++,count++ ){
        
            this->irData.voltages[count] = *itVoltages;       
            this->irData.ranges[count]   = *itRanges;
    } 

    pubIR.publish(irData);
}

bool EpuckDriver::GetIRPose2D(epuckDriver::GetIRPose2D::Request& req,
epuckDriver::GetIRPose2D::Response &res){

    ROS_INFO("GET POSE2DIR");
    if(exec){
        std::vector<EpuckInterface::Triple> epuckGeom;
        epuckGeom = this->epuckIR->GetGeometry();

        res.count = epuckGeom.size();    
        res.pose2d.resize(res.count);
 
        std::vector<EpuckInterface::Triple>::iterator it = epuckGeom.begin();

        for(int count=0; it < epuckGeom.end();count++,it++){
            res.pose2d[count].x = it->x;
            res.pose2d[count].y = it->y;
            res.pose2d[count].theta = it->theta;
        }
    }
    else
        ROS_INFO("Start first !!!");   
    
    return true;
}
/*---------------------------------------------------------------*/

/*----------------------CAMERA----------------------------*/
void EpuckDriver::CameraPublisher(){
    
    if( this->epuckCamera.get() ){
    
        unsigned char image[this->cameraData.image_count];
        this->epuckCamera->GetImage(image);
        
        for(int count=0;count < this->cameraData.image_count;count++) 
            this->cameraData.image[count] = image[count];

        pubCamera.publish(this->cameraData);
    }
}
/*--------------------------------------------------------*/

/*---------------------LED---------------------------------*/
void EpuckDriver::FrontLEDCallBack(const std_msgs::Bool::ConstPtr&  front){
        
    epuckSignal.lock();
    this->epuckLEDs->SetFrontLED(front->data);
    epuckSignal.unlock();
}

void EpuckDriver::BodyLEDCallBack(const std_msgs::Bool::ConstPtr& body){
    
    epuckSignal.lock();
    this->epuckLEDs->SetBodyLED(body->data);
    epuckSignal.unlock();
}

void EpuckDriver::RingLEDCallBack(const epuckDriver::BoolMsg::ConstPtr& ring){
     
    epuckSignal.lock();
    for(unsigned led=0; led<EpuckLEDs::RING_LEDS_NUM;led++)
        this->epuckLEDs->SetRingLED(led,ring->data[led]); 
    epuckSignal.unlock();
}
/*---------------------------------------------------------------*/

/*
int main(int argc,char **argv){

    char iD[] = {'1','2','3','4','5','6'};
    char *pathStandart = "/dev/refcomm\0";
    char pathDev[6][15];    
    int  numberEpucks;
    int  len = strlen(pathStandart);

    if(argc < 2){
        ROS_ERROR("Enter with the number of e-pucks");
        return -1;
    }
    else{
        numberEpucks = atoi(argv[1]); 
        
        if( !numberEpucks ){
            ROS_ERROR("Enter with number > 0 ");
            return -1;
        } 
        else{
            for(int i=0;i<numberEpucks;i++){    
                 strcpy(pathDev[i],pathStandart);
                 pathDev[i][len] = iD[i];
                 pathDev[i][len+1] = '\0';         
            }
        }    
    }

    ros::init(argc,argv,"epuckDriver");
    EpuckDriver *epucks[numberEpucks];
    ros::NodeHandle n;
    
    ros::Subscriber subVelocity[numberEpucks];   
    ros::Subscriber subCommand[numberEpucks];   
    ros::Subscriber subFrontLED[numberEpucks];   
    ros::Subscriber subBodyLED[numberEpucks];   
    ros::Subscriber subRingLED[numberEpucks];   
    
    ros::ServiceServer setOdometry[numberEpucks];
    ros::ServiceServer resetOdometry[numberEpucks];
    ros::ServiceServer getGeometry[numberEpucks];
    ros::ServiceServer getIRPose[numberEpucks];

    for(int i=0;i < numberEpucks ; i++){

        epucks[i] = new EpuckDriver(iD+i,pathDev[i]);

        subVelocity[i] = 
n.subscribe<geometry_msgs::Twist>(epucks[i]->WhereSubVelocity(),1,&EpuckDriver::VelocityCallBack,epucks[i]);
        subCommand[i] =
n.subscribe<epuckDriver::CommandSubscriberMsg>(epucks[i]->WhereSubCommand(),1,&EpuckDriver::CommandCallBack,epucks[i]);
        subFrontLED[i] =
n.subscribe<std_msgs::Bool>(epucks[i]->WhereSubFrontLED(),1,&EpuckDriver::FrontLEDCallBack,epucks[i]);
        subBodyLED[i] =
n.subscribe<std_msgs::Bool>(epucks[i]->WhereSubBodyLED(),1,&EpuckDriver::BodyLEDCallBack,epucks[i]);
        subFrontLED[i] =
n.subscribe<epuckDriver::BoolMsg>(epucks[i]->WhereSubRingLED(),1,&EpuckDriver::RingLEDCallBack,epucks[i]);
        
        setOdometry[i] =
n.advertiseService(epucks[i]->WhereServerSetOdom(),&EpuckDriver::SetOdometry,epucks[i]);
        resetOdometry[i] =      
n.advertiseService(epucks[i]->WhereServerResetOdom(),&EpuckDriver::ResetOdometry,epucks[i]);
        getGeometry[i] =   
n.advertiseService(epucks[i]->WhereServerGetGeom(),&EpuckDriver::GetGeometry,epucks[i]);
        getIRPose[i] =   
n.advertiseService(epucks[i]->WhereServerGetIRPose(),&EpuckDriver::GetIRPose2D,epucks[i]);
    }

    while(ros::ok()){
                
        for(int i=0;i < numberEpucks ; i++)
                epucks[i]->Publish();

        ros::spinOnce();
    }   

    return 0;
}
*/
